#include <csignal>
#include "iostream"
#include "include/ImagePublisher.h"
#include "include/ImageCatcher.h"
#include "ros/ros.h"
using namespace std;

StreamFactory sf;

void sigint_handler(int sig) {
	if (sig == SIGINT) {
		cout << "Aborting by Ctrl-C" << endl;
		cout << "Ending image reading thread." << endl;
		sf.EXIT_SIGNAL = true;
	}
}

void runWithImageCatcher(ImagePublisher& ip) {
//	ImageCatcher实现视频取帧功能
//	使用图像直接分解发送方式，无需运行 composite_publisher 节点
	ImageCatcher ic(2, 1920*2, 1080, "MJPG");
//	设置截图文件名
	ic.setFilename("photo_shot");
//	ImageCatcher显示视频
	ic.showVideoStream();

	while (ros::ok()) {
//		使用图像直接分解发送方式，publishAll发布origin, left, right
		ip.publishAll(ic);
		ros::spinOnce();
	}
}

void runWithStreamFactory(ros::NodeHandle& nh, ImagePublisher& ip) {
	//	使用StreamFactory从图片文件构造流
	string configFile;
	if (!nh.getParam("/image_publisher/stream_factory_config", configFile)) {
		cerr << "Failed to readFrame stream factory config." << endl;
		ROS_ERROR("Failed to readFrame stream factory config.");
	}

	cout << "Using stream config: " << configFile << endl;
	sf.init(configFile);
	sf.echo = false;
	cout << "   Left images from: " << sf.getLeftFrameDirectory() << endl;
	cout << "   Right images from: " << sf.getRightFrameDirectory() << endl;
	cout << "   Running at fps: " << sf.getFps() << "(" << sf.getDpf() << " dpf)" << endl;
//	从StreamFactory中显示图片流
//	vector<string> winNames = {"left", "right"};
//	sf.showFrames(winNames);
	Chrono chrono;
	std::chrono::duration<long, std::milli> timeCost{};

	signal(SIGINT, sigint_handler);
	while (ros::ok()) {
		if ((chrono.timeCostMilli() - timeCost) < std::chrono::milliseconds(50)) continue;
		timeCost = chrono.timeCostMilli();
		ros::param::set("/image_ok", true);
//		使用来自StreamFactory的图片流
		if (!sf.isEnabled()) {
			ROS_INFO("Stream disabled.\n");
			ros::param::set("/image_ok", false);
			break;
		}
		ip.publishStream(sf);
		ros::spinOnce();
	}
	ros::param::set("/image_ok", false);
	cout << "img_publisher exited." << endl;
}

void runWithRepublish(ImagePublisher& ip) {
	ip.republish("/composite_publisher/image_raw");
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "image_publisher");
	ros::NodeHandle nh;

/**=================================== ImageTopics ==========================================================*/
	ImageTopics imageTopics;
	imageTopics.originImageTopic = "/image_publisher/origin/image_raw";
	imageTopics.leftImageTopic = "/image_publisher/left/image_raw";
	imageTopics.rightImageTopic = "/image_publisher/right/image_raw";
	ImagePublisher ip(nh, imageTopics);

/**
 * Use stream from ImageCatcher (usb-camera)
 */
//	runWithImageCatcher(ip);


/*
 * Use stream from StreamFactory (generate stream from discrete image files)
 */
	runWithStreamFactory(nh, ip);

/**
 * Use stream form a composite image topic
 */
//	runWithRepublish(ip);
    return 0;
}
